"""
This is part of Human Robot Dynamics-Control, By Tourists.

Based on "OpenLoong Dynamics Control"
Transfer from mujoco to ISAAC SIM by IsaacLab

"""

import os
# from quill import start, LOG_INFO
import logging
# from eigen import EigenVectorXd

class DataLogger:
    def __init__(self, file_name):
        self.file_path = file_name
        self.folder, self.file_name = os.path.split(file_name)
        self.file_handler = logging.FileHandler(file_name, mode='w')
        self.file_handler.setFormatter(logging.Formatter("%(message)s"))
        self.logger = self.create_logger("logger", self.file_handler)
        self.start()
        self.col_count = 0
        self.rec_values = []
        self.rec_item_names = []
        self.rec_item_lengths = []
        self.rec_item_start_cols = []
        self.rec_item_end_cols = []
        self.is_item_data_in = []

    def start(self):
        # 初始化日志记录
        self.logger.info("Logging started")
    
    def create_logger(self, name, handler):
        logger = logging.getLogger(name)
        logger.setLevel(logging.INFO)
        formatter = logging.Formatter('%(asctime)s - %(name)s - %(levelname)s - %(message)s')
        handler.setFormatter(formatter)
        logger.addHandler(handler)
        return logger
    
    def add_item(self, name, length):
        if name in self.rec_item_names:
            raise ValueError(f"{name} has already been used!")
        self.rec_item_names.append(name)
        self.rec_item_lengths.append(length)
        self.rec_item_start_cols.append(self.col_count)
        self.rec_item_end_cols.append(self.col_count + length - 1)
        self.col_count += length

    def finish_item_adding(self):
        script_file = os.path.join(self.folder, "matlabReadDataScript.txt")
        with open(script_file, "w") as out_file:
            out_file.write("clear variables; close all\n")
            out_file.write(f"dataRec=load('{self.file_name}');\n")
            for i, name in enumerate(self.rec_item_names):
                out_file.write(f"{name}=dataRec(:,{self.rec_item_start_cols[i]+1}:{self.rec_item_end_cols[i]+1});\n")
            out_file.close()
        self.rec_values = [0.0] * self.col_count
        self.is_item_data_in = [False] * len(self.rec_item_names)

    def start_new_line(self):
        self.rec_values = [0.0] * self.col_count
        self.is_item_data_in = [False] * len(self.rec_item_names)

    def record_item_data(self, name, data):
        if name not in self.rec_item_names:
            raise ValueError(f"{name} has not been added!")
        index = self.rec_item_names.index(name)
        for i in range(self.rec_item_lengths[index]):
            # self.rec_values[self.rec_item_start_cols[index] + i] = data[i]
            self.rec_values[self.rec_item_start_cols[index] + i] = data
        self.is_item_data_in[index] = True

    def finish_line(self):
        missing_items = [name for name, is_in in zip(self.rec_item_names, self.is_item_data_in) if not is_in]
        formatted_values = []
        if missing_items:
            raise ValueError(f"{missing_items} have not been recorded values!")
        for value in self.rec_values:
            if isinstance(value, (int, float)):
                formatted_values.append(f"{value:.6e}")
            else:
                # 处理非数字类型的值
                formatted_values.append(str(value))
        formatted_line = ", ".join(formatted_values)
        self.logger.info(formatted_line)